/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class SteeringWheelDrive extends IterativeRobot {
    
    RobotDrive drive = new RobotDrive(8, 4, 1, 3);
    //RobotDrive drive = new RobotDrive(5,4,7,8);
    Joystick throttleStick = new Joystick(1);
    DriverStationEnhancedIO cypress = DriverStation.getInstance().getEnhancedIO();
    double turnRate = 0.0;
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        //drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        //drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
    
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
    
        try{
            if(cypress.getDigital(2)){
             turnRate = Util.deadZone(Util.constrain(Util.map(cypress.getAnalogIn(3), 0, 3.3, -1, 1), -1, 1), -0.1, 0.1) ;
           }
            else{
            turnRate = Util.deadZone(Util.constrain(Util.map(cypress.getAnalogIn(3), 0, 3.3, -2, 2), -1, 1), -0.1, 0.1) ;     
           }
           }
    catch(DriverStationEnhancedIO.EnhancedIOException ex){
        turnRate = 0.0;
    }
    
    //turnRate = Util.deadZone(throttleStick.getX(), -0.1, 0.1);
    drive.arcadeDrive(Util.deadZone(throttleStick.getY(), -0.05, 0.05), -turnRate);
    System.out.println( turnRate);
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    
    }
    
}
